package com.rockwell.robot2015.model.solvers;

import java.util.ArrayList;

import com.rockwell.robot2015.model.IRobotLogger;
import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.RobotLocation;
import com.rockwell.robot2015.model.command.RobotCommand;
import com.rockwell.robot2015.model.sensors.IRobotSensors;

public class PredefinedMazeSolver implements IMazeSolver {
	private ArrayList<RobotLocation> m_intersections;
	private ArrayList<RobotAction> m_actions;
	
	private IMazeSolver m_fallbackSolver;
	
	private IRobotLogger m_logger;
	
	private int m_index = 0;
	
	private boolean m_hasError = false;
	
	private RobotCommand m_com = new RobotCommand();
	
	public PredefinedMazeSolver(ArrayList<RobotLocation> intersections, 
			ArrayList<RobotAction> directions, IMazeSolver fallbackSolver, IRobotLogger logger) {
		// assert(m_intersections.length == m_directions.length)
		
		m_intersections = intersections;
		m_actions = directions;
		m_fallbackSolver = fallbackSolver;
		m_logger = logger;
	}

	@Override
	public void reset(Robot robot) {
		m_index = 0;
	}
	
	@Override
	public void onJunctionReached(Robot robot) {
		m_index++;
	}

	@Override
	public void onFrontWallReached(Robot robot) {
		IRobotSensors sensor = robot.getSensor();
		
		if (sensor.detectLeftWall() && sensor.detectRightWall()) {
			// dead end
			m_index++;
		}
	}
	
	@Override
	public void move(Robot robot) {
		IRobotSensors sensor = robot.getSensor();
		
		if (sensor.detectFrontWall() && sensor.detectLeftWall() && sensor.detectRightWall()) {
			robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);
		}
		
		RobotAction ra = m_actions.get(m_index);
		
		switch (ra) {
		case TurnLeft:
			if (!sensor.detectLeftWall()) {
				robot.sendCommand(RobotAction.MoveWithoutSensor,
						LEFT_TURN_CLEARANCE);
				robot.sleep(Robot.MOTION_SETTLING_TIME);

				robot.turnLeft();
				robot.sleep(Robot.MOTION_SETTLING_TIME);
			}
			else {
				hasError(true);
			}
			break;
			
		case TurnRight:
			if (!sensor.detectRightWall()) {
				robot.sendCommand(RobotAction.MoveWithoutSensor,
						LEFT_TURN_CLEARANCE);
				robot.sleep(Robot.MOTION_SETTLING_TIME);

				robot.turnRight();
				robot.sleep(Robot.MOTION_SETTLING_TIME);
			}
			else {
				hasError(true);
			}
			break;
			
		case TurnBack:
			robot.turnBack();
			robot.sleep(Robot.MOTION_SETTLING_TIME);
			break;
			
		default:
			if (!sensor.detectRightWall()) {
				robot.sendCommand(RobotAction.Jog2);
			}
			break;
		}
	}

	private void hasError(boolean hasError) {
		if (!m_hasError && hasError && m_logger != null) {
			m_logger.log("Predefine solver failed");
		}
		
		m_hasError = hasError;
	}
}
